#include "TOUCAN.H"

#include "TOUCANDataObjects.H"

#if 1

#include "DriverStation.h"
#include "AnalogChannel.h"
#include "Timer.h"
#include "Utility.h"
#include "WPIStatus.h"
#include "NetworkCommunication/FRCComm.h"
#include <strLib.h>
#include "Task.h"
#include "AxisCamera.h" 
#include "TrackAPI.h"
#include "PIDController.h"


TOUCAN* TOUCAN::m_pInstance = NULL;


typedef union _brokenWORD
{
	int16 u16;
	char bytes[2];
}brokenWORD;

void Swap(int16 & var)
{
	brokenWORD b;
	b.u16 = var;
	char cSwap = b.bytes[1];
	b.bytes[1] = b.bytes[0];
	b.bytes[0] = cSwap;
	var = b.u16;
}

/**
 * TOUCAN contructor.
 * 
 * This is only called once the first time GetInstance() is called
 */
TOUCAN::TOUCAN()  : m_obTask("TOUCAN",(FUNCPTR)InitTask)
{
	m_bDie = false;

	memset(&obInPacketRaw,0,sizeof(obInPacketRaw));
	memset(&obInPacket,0,sizeof(obInPacket));
	memset(&obOutPacketRaw,0,sizeof(obOutPacketRaw));
	memset(&obOutPacket,0,sizeof(obOutPacket));

	memset(&obJagPars,0,sizeof(obJagPars));
	
	obOutPacket.iSig = SIG_JAG_INPUT_PACKET;
	obInPacket.iSig = SIG_JAG_OUTPUT_PACKET;
	obOutPacket.iByteLen = sizeof(obOutPacket);
	obInPacket.iByteLen = sizeof(obInPacket);

	obOutPacketRaw.iSig = SIG_JAG_INPUT_PACKET;
	obInPacketRaw.iSig = SIG_JAG_OUTPUT_PACKET;
	obOutPacketRaw.iByteLen = sizeof(obOutPacketRaw);
	obInPacketRaw.iByteLen = sizeof(obInPacketRaw);

	m_ThrID = semMCreate(SEM_Q_PRIORITY);

	if (!m_obTask.Start((INT32)this))
	{
		wpi_fatal(DriverStationTaskError);
	}
}

TOUCAN::~TOUCAN()
{
	m_pInstance = NULL;
	
	Discard();
}
/**
 * Return a pointer to the singleton TOUCAN.
 */
TOUCAN* TOUCAN::GetInstance()
{
	if (m_pInstance == NULL)
		m_pInstance = new TOUCAN();
	return m_pInstance;
}

void TOUCAN::InitTask(TOUCAN * pThis)
{
	pThis->Run();
}
void TOUCAN::Swap(SOutputPacket & Packet)
{
	for(unsigned int i=0;i<sizeof(Packet)/2;++i)
		::Swap(Packet.Words[i]);
}	
void TOUCAN::Swap(SInputPacket & Packet)
{
	for(unsigned int i=0;i<sizeof(Packet)/2;++i)
		::Swap(Packet.Words[i]);
}

unsigned short TOUCAN::CheckSum(unsigned short * data,
											unsigned long byte_len)
{
	unsigned int i=0;
	unsigned short c=0;
	unsigned short d=0;
	
	c = 0;
	for(;i<byte_len/2;++i)
	{
		c += data[i];
	}

	return ~c + 1;
}
void TOUCAN::Discard()
{
	CRITICAL_REGION(m_ThrID);
	m_bDie = true;
	END_REGION;
	
	m_obTask.Stop();
	
}
void TOUCAN::Run()
{
	bool bDie = false;
	
	unsigned long len;

	RxUDPSocket obRx;
	TxUDPSocket obTx;
	
	while(bDie == false)
	{
		len = sizeof(obInPacket);
		len = obRx.Receive((char*)&obInPacketRaw,len);
		if(len)
		{
			Swap(obInPacketRaw);
			unsigned short chk = CheckSum((unsigned short*)&obInPacketRaw,sizeof(obInPacketRaw));
			if(chk == 0)
			{
				obInPacket = obInPacketRaw;
			}
		}

//		mapJagParams_t::iterator i = m_mapJagParams.begin();
//		obOutPacket.iJagCount = 0;
//		for(;i!= m_mapJagParams.end();++i)
//		{
//			obOutPacket.obJagParams[ obOutPacket.iJagCount++ ] = i->second;
//			
//			if(obOutPacket.iJagCount == MAX_NUM_JAGS)
//				break;
//				
//		}

		obOutPacket.iJagCount = MAX_NUM_JAGS;
		obOutPacketRaw = obOutPacket;
		obOutPacketRaw.iCRC = 0;
		obOutPacketRaw.iCRC = CheckSum((unsigned short*)&obOutPacketRaw,sizeof(obOutPacketRaw));
		Swap(obOutPacketRaw);
		obTx.Send((char*)&obOutPacketRaw, sizeof(obOutPacketRaw));
		
		taskDelay(10);
		
		CRITICAL_REGION(m_ThrID);
		bDie = m_bDie;
		END_REGION;
	}
}

TOUCAN_Error_t TOUCAN::SetVoltage(JagID_t id,float fVoltage)
{
	if((id-1) >= MAX_NUM_JAGS)
		return iTOUCAN_ErrorInvalidID;
	
	if(fVoltage > 12)
		fVoltage = 12;
	else if(fVoltage < -12)
		fVoltage = -12;
	
	signed short ss = (signed short)(fVoltage/12.0f * 32767);
	obOutPacket.obJagParams[id-1].iSetVoltage = ss;
	
	return 0;
}


#endif
